classdef IMUMeasCorrTest_Inc < matlab.unittest.TestCase
    
    properties (TestParameter)
        Diff2TrueRatio = {0.05};
        OuterIterStepNum = {5};
        InnerIterStepNum = {4};
    end
    
    methods (Test)
        function vldIMUMeasCorr_Inc(testCase, Diff2TrueRatio, OuterIterStepNum, InnerIterStepNum)
            % 验证增量形式的比力及角速度修正量计算
            
            %% 参数设置
            nSamp = 1000;
            Tl = 0.005; % 5ms
            DupsilonBlTrue = randi([-20, 20], [3 nSamp]) * 9.8 * Tl; % 20g
            DalphaBlTrue = randi([-100, 100], [3 nSamp+1]) / 180 * pi * Tl; % 100°/s
            DalphaBlTrue(:, 2:end) = DalphaBlTrue(:, 1:(end-1)) + randi([-200, 200], [3 nSamp])/180*pi*Tl^2; % 200°/s^2
            DalphaBl1True = DalphaBlTrue(:, 1:(end-1));
            DalphaBlTrue = DalphaBlTrue(:, 2:end);
            IMUParTrue = genrandimupar(nSamp, true, true);
            
            IMUParTrueEquiv = genrandimupar(nSamp, true);
            [IMUParDiff, IMUParAppr] = genimupardiff(nSamp, IMUParTrue, IMUParTrueEquiv, Diff2TrueRatio);
            
            %% 计算加入误差的输出值并计算比力及角速度近似值
            [~, DupsilonTildeSl] = accerror_inc(DupsilonBlTrue, IMUParTrue.acc, Tl, DalphaBlTrue, DalphaBl1True(:, 1), false);
            DupsilonTildeOutSl = IMUParTrue.acc.KScal0 .* DupsilonTildeSl;
            [~, DalphaTildeSl] = gyroerror_inc(DalphaBlTrue, IMUParTrue.gyro, Tl, DupsilonBlTrue, false);
            DalphaTildeOutSl = IMUParTrue.gyro.KScal0 .* DalphaTildeSl;
            [DupsilonBlAppr, DalphaBlAppr] = imuout2in_inc(DupsilonTildeOutSl, DalphaTildeOutSl, IMUParAppr, Tl, DalphaBl1True, int8([1; 1]), OuterIterStepNum, InnerIterStepNum);
            DupsilonBlCorrRef = DupsilonBlTrue - DupsilonBlAppr;
            DalphaBlCorrRef = DalphaBlTrue - DalphaBlAppr;
            
            %% 偏微分算法计算修正量
            [DupsilonBlCorrCalc_pdiff, DalphaBlCorrCalc_pdiff] = imumeascorr_pdiff_inc(IMUParDiff, IMUParAppr, DupsilonBlAppr, DalphaBlAppr, Tl, DalphaBlTrue);
            
            DupsilonBlCorrErr_pdiff = (DupsilonBlCorrCalc_pdiff-DupsilonBlCorrRef)' / 9.8 * 1e6 / Tl;
            DupsilonBlCorrRelatErr_pdiff = relatdiff(DupsilonBlCorrCalc_pdiff, DupsilonBlCorrRef)' * 100;
            disp('偏微分算法DupsilonBl修正量误差绝对值最大值（μg）');
            disp(max(abs(DupsilonBlCorrErr_pdiff), [], 1));
            disp('偏微分算法DupsilonBl修正量误差绝对值平均值（μg）');
            disp(mean(abs(DupsilonBlCorrErr_pdiff), 1));
            disp('偏微分算法DupsilonBl修正量相对误差绝对值平均值（%）');
            disp(mean(abs(DupsilonBlCorrRelatErr_pdiff), 1));
            testCase.verifyLessThanOrEqual(mean(abs(DupsilonBlCorrRelatErr_pdiff)), 0.5);
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
            
            DalphaBlCorrErr_pdiff = (DalphaBlCorrCalc_pdiff-DalphaBlCorrRef)' / pi * 180 * 3600 / Tl;
            DalphaBlCorrRelatErr_pdiff = relatdiff(DalphaBlCorrCalc_pdiff, DalphaBlCorrRef)' * 100;
            disp('偏微分算法DalphaBl修正量误差绝对值最大值（°/h）');
            disp(max(abs(DalphaBlCorrErr_pdiff), [], 1));
            disp('偏微分算法DalphaBl修正量误差绝对值平均值（°/h）');
            disp(mean(abs(DalphaBlCorrErr_pdiff), 1));
            disp('偏微分算法DalphaBl修正量相对误差绝对值平均值（%）');
            disp(mean(abs(DalphaBlCorrRelatErr_pdiff), 1));
            testCase.verifyLessThanOrEqual(mean(abs(DalphaBlCorrRelatErr_pdiff)), 0.5);
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
            
            %% 1阶算法计算修正量
            DupsilonBlCorrCalc_1stod = accerror_inc(DupsilonBlAppr, IMUParDiff.acc, Tl, DalphaBlAppr, DalphaBl1True, true);
            DalphaBlCorrCalc_1stod = gyroerror_inc(DalphaBlAppr, IMUParDiff.gyro, Tl, DupsilonBlAppr, true);
            
            DupsilonBlCorrErr_1stod = (DupsilonBlCorrCalc_1stod-DupsilonBlCorrRef)' / 9.8 * 1e6 / Tl;
            DupsilonBlCorrRelatErr_1stod = relatdiff(DupsilonBlCorrCalc_1stod, DupsilonBlCorrRef)' * 100;
            disp('1阶算法DupsilonBl修正量误差绝对值最大值（μg）');
            disp(max(abs(DupsilonBlCorrErr_1stod), [], 1));
            disp('1阶算法DupsilonBl修正量误差绝对值平均值（μg）');
            disp(mean(abs(DupsilonBlCorrErr_1stod), 1));
            disp('1阶算法DupsilonBl修正量相对误差绝对值平均值（%）');
            disp(mean(abs(DupsilonBlCorrRelatErr_1stod), 1));
            testCase.verifyLessThanOrEqual(mean(abs(DupsilonBlCorrRelatErr_1stod)), 5);
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
            
            DalphaBlCorrErr_1stod = (DalphaBlCorrCalc_1stod-DalphaBlCorrRef)' / pi * 180 * 3600 / Tl;
            DalphaBlCorrRelatErr_1stod = relatdiff(DalphaBlCorrCalc_1stod, DalphaBlCorrRef)' * 100;
            disp('1阶算法DalphaBl修正量误差绝对值最大值（°/h）');
            disp(max(abs(DalphaBlCorrErr_1stod), [], 1));
            disp('1阶算法DalphaBl修正量误差绝对值平均值（°/h）');
            disp(mean(abs(DalphaBlCorrErr_1stod), 1));
            disp('1阶算法DalphaBl修正量相对误差绝对值平均值（%）');
            disp(mean(abs(DalphaBlCorrRelatErr_1stod), 1));
            testCase.verifyLessThanOrEqual(mean(abs(DalphaBlCorrRelatErr_1stod)), 5);
            disp('~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~');
        end
    end
end